/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    state_Jump.h
  * @author  Mentos Seetoo
  * @brief   Finite State Machine for jumping up.
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
#ifndef _FSM_JUMP_H
#define _FSM_JUMP_H
/* Includes ------------------------------------------------------------------*/
#include "FSM.h"
/* Types define --------------------------------------------------------------*/
namespace Quadruped
{
  /**
 * @brief Class for jump up state.
 * 
 */
template <typename T>
class FSM_Jump : public FSM_Base<T>
{
public:
    FSM_Jump(GaitScheduler<T> *p_gait,
            StateEstimator<T> *p_estimator,
            DesiredCoMState<T> *p_command,
            Dynamic<T> *p_dynamic,
            BalanceController<T> *p_controller,
            T run_period);

    void RunState();
    int Transition(FSM_Base<T>* target_state);

private:
    Dynamic<T>            *p_Dynamic;
    DesiredCoMState<T>    *p_CoMCommand;
    StateEstimator<T>     *p_StateEstimator;
    GaitScheduler<T>      *p_GaitScheduler;
    BalanceController<T>  *p_BalanceController;
    Vec3<T> pBeforeJumpPos[4],pAfterJumpPos[4],pStandPos[4],pBoundJump[4],pRetractPos[4],pFallPos[4];
    Vec3<T> pDesiredFoot[4];
    int once_use = 4 ; //4 legs, thus once_use must be 4
};

} // namespace Quadruped
#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/